#ifndef GRIPPER_COMMUNICATOR_HPP
#define GRIPPER_COMMUNICATOR_HPP

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/bool.hpp>
#include "gripper_control/gripper_json.hpp"
#include <string>
#include <memory>
#include <chrono>
#include <map>
#include <utility>

namespace gripper_control { 

    class GripperCommunicator : public rclcpp::Node 
    { 
        public:
            explicit GripperCommunicator(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());

        private:

            // MQTT通信相关
            rclcpp::Subscription<std_msgs::msg::String>::SharedPtr mqtt_feedback_sub_; 
            rclcpp::Publisher<std_msgs::msg::String>::SharedPtr mqtt_command_pub_;    
            rclcpp::Publisher<std_msgs::msg::String>::SharedPtr error_pub_;           

            // ROS通信相关
            // 订阅传入的放射源执行动作
            rclcpp::Subscription<std_msgs::msg::String>::SharedPtr source_type_sub_;
            rclcpp::Subscription<std_msgs::msg::String>::SharedPtr action_type_sub_;

            rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr local_reached_sub_;  
            rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr completion_pub_;        

            // 参数
            std::string mqtt_feedback_topic_; 
            std::string mqtt_command_topic_;
            std::string error_topic_;
            std::string source_type_topic_;
            std::string action_type_topic_;
            std::string local_reached_topic_;
            std::string gripper_completion_topic_;

            // 映射表
            std::map<std::pair<std::string, std::string>, int>command_map_;


            // 状态管理
            bool has_local_reached_;
            std::string source_type_;
            std::string action_type_;

            int operate_command_;
            int last_operate_command_;
            bool has_gripper_got_command_;
            int gripper_current_state_;
            bool waiting_for_response_;
            int gripper_initial_state_;
            bool error_code_;
            bool operation_completed_;
            

            // JSON转换实例
            GripperJsonConverter command_json_;
            GripperJsonConverter feedback_json_;
            GripperJsonConverter error_json_;
            rclcpp::Time command_time_;

            
            // 定时器
            rclcpp::TimerBase::SharedPtr response_timer_;
            rclcpp::TimerBase::SharedPtr completion_timer_;

            // 回调函数
            void localReachingCallback(const std_msgs::msg::Bool::SharedPtr msg);
            void mqttFeedbackCallback(const std_msgs::msg::String::SharedPtr msg);

            // 内部函数
            void initializeCommandMap();    // 初始化映射表
            void updateOperateCommand();    // 根据source_type_和action_type_更新operate_command_

            void sendCommand();
            void startWaiting();
            void checkResponse();
            void checkCompletion();
            void publishCompletion();
            void publishError();

    };
    
}

#endif